#include "SteeringAlignBehaviour.h"

#include <iostream>
using namespace std;

SteeringAlignBehaviour::SteeringAlignBehaviour(double stopRadius, double slowRadius) :
     m_stopRadius(stopRadius), m_slowRadius(slowRadius){
}

SteeringAlignBehaviour::SteeringAlignBehaviour(SteeringAlignBehaviour&  cSAB) :
    m_stopRadius(cSAB.m_stopRadius), m_slowRadius(cSAB.m_slowRadius){
}

SteeringAlignBehaviour& SteeringAlignBehaviour::operator=(SteeringAlignBehaviour& cSAB){
    // impossible - const values
    cout << "Why do u want to do zat?" << endl;
}

SteeringAlignBehaviour::~SteeringAlignBehaviour(){
    cout << "Destruction SteeringAlignBehaviour" << endl;
}

void SteeringAlignBehaviour::stop(SteeringBehaviourOutput& output, double angularSpeed, double maxAngularAcc) {
    double acc = -std::min(fabs(angularSpeed), maxAngularAcc);
    output.setAngular(signum((angularSpeed)*acc));
}

void SteeringAlignBehaviour::brake(SteeringBehaviourOutput& output, double angularSpeed, double maxAngularAcc, double rotation) {
    double t = angularSpeed / rotation; //this.timeToTarget;
    output.setAngular(-angularSpeed * t);
}

SteeringBehaviourOutput* SteeringAlignBehaviour::runAlign(Vector2D orientation, double angularSpeed, double maxAngularAcc, Vector2D target) {
    SteeringBehaviourOutput *output = new SteeringBehaviourOutput();

    // Get the naive direction to target, with an angle representation
    double rotation = signedAngle(orientation,target);


    if (rotation!=0 && angularSpeed==0) {
            output->setAngular(signum(rotation) * std::min(maxAngularAcc,rotation));
    }
    else {            
            // the result is already mapped to (-pi,pi) interval with signedAngle()
            double rotationSize = fabs(rotation);

            // Check if we are there
            if (rotationSize<this->m_stopRadius) {
                    // try to stop
                    stop(*output, angularSpeed, maxAngularAcc);
                    std::cout<<"STOOOOOOOOOOOOOP"<<std::endl;
            }
            else if (rotationSize<this->m_slowRadius) {
                std::cout<<"BRAAAAAAAAAAAAAAKE"<<std::endl;
                    brake(*output, angularSpeed, maxAngularAcc, rotationSize);
            }
            else {
                    std::cout<<"ROOOTATE ::: "<<signum(rotation)<<" :: "<<maxAngularAcc<<std::endl;
                    // Turn as fast as possible
                    output->setAngular(signum(rotation) * maxAngularAcc);
            }
    }

    return output;
}
